#include "StdAfx.h"
#include "Dijkstras.h"
#include "math.h"

CDijkstras::CDijkstras(CNodePtrVec* vecNodes, int(*dist)(CNode*, CNode*))
{
    m_vecNodes = *vecNodes; // This creates a copy of vecNodes
    m_vecNodesCopy = *vecNodes;
    m_fxDist = dist;
}

CDijkstras::~CDijkstras(void)
{
}

void CDijkstras::SetSource(CNode* source)
{
    // Reset list    
    m_vecNodes = m_vecNodesCopy;
    for (CNodePtrVecIter it = m_vecNodes.begin(); it != m_vecNodes.end(); ++it)
    {
        (*it)->m_dfs = INT_MAX;
        (*it)->m_prev = NULL;
    }

    // Set start (dfs of 0 means it will be chosen first)
    m_source = source;
	m_source->m_dfs = 0;

    // Run the algorithm now that we have a source!
    Run();
}

CPath* CDijkstras::GetPath(CNode *source, CNode *dest)
{
    SetSource(source);
    return GetPathTo(dest);
}

CPath* CDijkstras::GetPathTo(CNode* dest)
{
    CPath* path = new CPath(m_source, dest);

	CNode* previous = dest;
	while (previous)
	{
        path->GetPath()->insert(path->GetPath()->begin(), previous);
		previous = previous->m_prev;
	}

    return path;
}

void CDijkstras::Run()
{
    // Main loop
	while (m_vecNodes.size() > 0)
	{
        // Get untouched node with smallest distance
		CNode* smallest = GetSmallestNode();     
        
        // Get list of adjacent nodes
        CNodePtrVec vecAdj;
		GetAdjacentNodes(smallest, vecAdj);

        // Loop through adjacent nodes and updaet their distances.
		for (CNodePtrVecIter it = vecAdj.begin(); it != vecAdj.end(); ++it)
		{
			CNode* cnode = *it;
			int distance = m_fxDist(smallest, cnode) + smallest->m_dfs;
			
			if (distance < cnode->m_dfs)
			{
				cnode->m_dfs = distance;
				cnode->m_prev = smallest;
			}
		}
	}
}

CNode* CDijkstras::GetSmallestNode()
{	
	CNode* smallest = NULL;
    CNodePtrVecIter itSmall;

	for (CNodePtrVecIter it = m_vecNodes.begin(); it != m_vecNodes.end(); ++it)
	{
		CNode* current = *it;
		if (!smallest || current->m_dfs < smallest->m_dfs)
		{
			smallest = current;
			itSmall = it;
		}
	}

	m_vecNodes.erase(itSmall);
	return smallest;
}

void CDijkstras::GetAdjacentNodes(CNode* node, CNodePtrVec& vecAdjNodes)
{
    if (node->m_north)
        vecAdjNodes.push_back(node->m_north);

    if (node->m_east)
        vecAdjNodes.push_back(node->m_east);

    if (node->m_south)
        vecAdjNodes.push_back(node->m_south);

    if (node->m_west)
        vecAdjNodes.push_back(node->m_west);

}

bool CDijkstras::NodeInList(CNode* node)
{
	for (CNodePtrVecIter it = m_vecNodes.begin(); it != m_vecNodes.end(); ++it)
	{
		if (*it == node)
		{
			return true;
		}
	}

	return false;
}


